package aos.server.robot;

import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import gnu.io.UnsupportedCommOperationException;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;

import org.apache.log4j.Logger;

import aos.server.ServerProperties;

public class RobotController {
	
	Logger logger = Logger.getLogger(RobotController.class);
	
	static InputStream input;
	static OutputStream output;
	
	public static final int SERIAL_PORT_TIME_OUT = 4000;

	

	public RobotController() {

		try {
			
			String commPort = ServerProperties.getInstance().getProperty(ServerProperties.COMM_PORT);
			logger.info("Attempt connexion to port " + commPort);
			
			CommPortIdentifier portId = CommPortIdentifier.getPortIdentifier(commPort);

			SerialPort port = (SerialPort) portId.open("serial talk", SERIAL_PORT_TIME_OUT);

			input = port.getInputStream();

			output = port.getOutputStream();

			port.setSerialPortParams(ServerProperties.getInstance().getIntegerProperties(ServerProperties.COMM_RATE),
					ServerProperties.getInstance().getIntegerProperties(ServerProperties.COMM_DATABITS),
					ServerProperties.getInstance().getIntegerProperties(ServerProperties.COMM_STOPBITS),
					ServerProperties.getInstance().getIntegerProperties(ServerProperties.COMM_PARITY)
					);
			
			logger.info("Connected to port " + commPort);

		}catch (PortInUseException e) {
			logger.fatal("System exit with error", e);
			System.exit(-1);
		} catch (NoSuchPortException e) {
			logger.fatal("System exit with error", e);
			System.exit(-1);
		} catch (UnsupportedCommOperationException e) {
			logger.fatal("System exit with error", e);
			System.exit(-1);
		} catch (IOException e) {
			logger.fatal("System exit with error", e);
			System.exit(-1);
		}
	}
	
	public void updatePosition(double xhead, double yhead, double xwheel, double ywheel) {
		if(xhead < 0.0 || xhead > 1.0 || yhead < 0.0 || yhead > 1.0 || xwheel < 0.0 || xwheel > 1.0 || ywheel < 0.0 || ywheel > 1.0) {
			logger.error("Update position out of range");
			return;
		}
		try {
			output.write((byte) 0xFA);
			output.write((byte) (xhead * 180));
			// robot head limited from 0deg to 126deg 
			output.write((byte) (126 - (yhead * 126)));
			double ampl = Math.sqrt((Math.pow(xwheel - 0.5, 2) + Math.pow(ywheel - 0.5, 2)));
			double phi = Math.atan2(0.5 - xwheel, 0.5 - ywheel);
			double motorA = 2 * (ampl * Math.sin(phi) * 255 + ampl * Math.cos(phi) * 255);
			double motorB = 2 * (ampl * Math.sin(phi) * 255 - ampl * Math.cos(phi) * 255);
			logger.debug("A: " + ampl + " Phi: " + phi + " motorA:" + motorA + " motorB:" + motorB);
			if(motorA > 0.0) {
				output.write((byte) (0));
			} else {
				output.write((byte) (1));
			}
			output.write((byte) (Math.min(Math.abs(motorA), 255)));
			if(motorB > 0.0) {
				output.write((byte) (1));
			} else {
				output.write((byte) (0));
			}
			output.write((byte) (Math.min(Math.abs(motorB), 255)));
			logger.info("Position update sent to robot");
		} catch (IOException e) {
			logger.error("Issue when trying to send position update to robot", e);
		}
		
	}

}